
#include "gsl_ext_function.h"
#include "gsl_ext_multider.h"

#include <gsl/gsl_blas.h>
#include <gsl/gsl_machine.h>

static const double H_CENTRAL = GSL_ROOT3_DBL_EPSILON;
static const double H_FORWARD = GSL_SQRT_DBL_EPSILON;

gsl_ext_multider_evaluator_params *
gsl_ext_multider_evaluator_alloc(int n,
                                 void *params)
{
  gsl_ext_multider_evaluator_params *e = 
    malloc(sizeof(gsl_ext_multider_evaluator_params));

  e->n = n;
  e->x_hi   = gsl_vector_alloc(n);
  e->x_hj   = gsl_vector_alloc(n);
  e->x_hihj = gsl_vector_alloc(n);
  e->f_hi   = gsl_vector_alloc(n);

  return e;
}

void 
gsl_ext_multider_evaluator_free(gsl_ext_multider_evaluator_params *e)
{
  gsl_vector_free(e->f_hi);
  gsl_vector_free(e->x_hihj);
  gsl_vector_free(e->x_hj);
  gsl_vector_free(e->x_hi);
  
  free(e);
}

void 
gsl_ext_multider_central_difference_df(const gsl_vector *x,
                                       void *params,
                                       gsl_vector *g)
{
  gsl_ext_multimin_function_params *p = 
    (gsl_ext_multimin_function_params *)params;
  gsl_ext_multider_evaluator_params *e = 
    (gsl_ext_multider_evaluator_params *)p->df_evaluator;
  
  int i;
  double fxp, fxm;
  double xi;
  
  gsl_blas_dcopy(x, e->x_hi);
  
  for(i = 0; i < e->n; i++)
    {
      xi = gsl_vector_get(x, i);
      gsl_vector_set(e->x_hi, i, xi + H_CENTRAL);
      fxp = (*p->f)(e->x_hi, params);
      gsl_vector_set(e->x_hi, i, xi - H_CENTRAL);
      fxm = (*p->f)(e->x_hi, params);
      gsl_vector_set(e->x_hi, i, xi);

      gsl_vector_set(g, i, (fxp - fxm) / (2.0 * H_CENTRAL));
    }
}

static void forward_difference_df(const gsl_vector *x,
                                  double fx,
                                  void *params,
                                  gsl_vector *g)
{
  gsl_ext_multimin_function_params *p = 
    (gsl_ext_multimin_function_params *)params;
  gsl_ext_multider_evaluator_params *e = 
    (gsl_ext_multider_evaluator_params *)p->df_evaluator;

  int i;
  double fxp;
  double xi;

  gsl_blas_dcopy(x, e->x_hi);

  for(i = 0; i < e->n; i++)
  {
    xi = gsl_vector_get(x, i);
    gsl_vector_set(e->x_hi, i, xi + H_FORWARD);
    fxp = (*p->f)(e->x_hi, params);
    gsl_vector_set(e->x_hi, i, xi);
    gsl_vector_set(g, i, (fxp - fx) / H_FORWARD);
  }
}

void 
gsl_ext_multider_forward_difference_df(const gsl_vector *x,
                                     void *params,
                                     gsl_vector *g)
{
  gsl_ext_multimin_function_params *p = 
    (gsl_ext_multimin_function_params *)params;

  double fx = (*p->f)(x, params);
  forward_difference_df(x, fx, params, g);
}

void 
gsl_ext_multider_forward_difference_fdf(const gsl_vector *x,
                                        void *params,
                                        double *f,
                                        gsl_vector *g)
{
  gsl_ext_multimin_function_params *p = 
    (gsl_ext_multimin_function_params *)params;

  *f = (*p->f)(x, params);
  forward_difference_df(x, *f, params, g);
}

static void forward_difference_d2f(const gsl_vector *x,
                                   double fx,
                                   gsl_vector *f_hi,
                                   void *params,
                                   gsl_matrix *d2f)
{
  int i, j;

  gsl_ext_multimin_function_params *p = 
    (gsl_ext_multimin_function_params *)params;
  gsl_ext_multider_evaluator_params *e = 
    (gsl_ext_multider_evaluator_params *)p->d2f_evaluator;

  gsl_blas_dcopy(x, e->x_hi);
  gsl_blas_dcopy(x, e->x_hj);
  gsl_blas_dcopy(x, e->x_hihj);

  for(i = 0; i < e->n; i++)
  {
    /* x + he_i */
    gsl_vector_set(e->x_hi, i, gsl_vector_get(x, i) + H_FORWARD);
    
    for(j = 0; j <= i; j++)
    {
      /* x + he_j */
      gsl_vector_set(e->x_hj, j, gsl_vector_get(x, j) + H_FORWARD);
      
      if(i != j)
      {
        /* x + he_i + he_j */
        gsl_vector_set(e->x_hihj, i, gsl_vector_get(e->x_hi, i));
        gsl_vector_set(e->x_hihj, j, gsl_vector_get(e->x_hj, j));
      }
      else
        gsl_vector_set(e->x_hihj, i, gsl_vector_get(x, i) + 2.0 * H_FORWARD);

      gsl_matrix_set(d2f, i, j, 
                     (p->f(e->x_hihj, params) - gsl_vector_get(f_hi, i) - 
                     (gsl_vector_get(f_hi, j) - fx)) / 
                     (H_FORWARD * H_FORWARD));

	    gsl_vector_set(e->x_hj, i, gsl_vector_get(x, i)); // TODO: wrong index?!
      gsl_vector_set(e->x_hihj, i, gsl_vector_get(x, i));
      gsl_vector_set(e->x_hihj, j, gsl_vector_get(x, j));
    }
    
    gsl_vector_set(e->x_hi, i, gsl_vector_get(x, i));
  }
  
  for(i = 0; i < e->n; i++)
    for(j = i + 1; j < e->n; j++)
      gsl_matrix_set(d2f, i, j, gsl_matrix_get(d2f, j, i));
}

void 
gsl_ext_multider_forward_difference_d2f(const gsl_vector *x, 
                                      void *params, 
                                      gsl_matrix *d2f)
{
  int i;

  gsl_ext_multimin_function_params *p = 
    (gsl_ext_multimin_function_params *)params;
  gsl_ext_multider_evaluator_params *e = 
    (gsl_ext_multider_evaluator_params *)p->d2f_evaluator;

  double fx = p->f(x, params);
  
  gsl_blas_dcopy(x, e->x_hi);
  for(i = 0; i < e->n; i++)
  {
    gsl_vector_set(e->x_hi, i, gsl_vector_get(x, i) + H_FORWARD);
    gsl_vector_set(e->f_hi, i, p->f(e->x_hi, params));
    gsl_vector_set(e->x_hi, i, gsl_vector_get(x, i));
  }
  
  forward_difference_d2f(x, fx, e->f_hi, params, d2f);
}

void 
gsl_ext_multider_forward_difference_fd2f(const gsl_vector *x,
                                       void *params,
                                       double *f,
                                       gsl_vector *df,
                                       gsl_matrix *d2f)
{
  int i;
  
  gsl_ext_multimin_function_params *p = 
    (gsl_ext_multimin_function_params *)params;
  gsl_ext_multider_evaluator_params *e = 
    (gsl_ext_multider_evaluator_params *)p->d2f_evaluator;
  
  *f = p->f(x, params);
  for(i = 0; i < e->n; i++)
  {
    gsl_vector_set(e->x_hi, i, gsl_vector_get(x, i) + H_FORWARD);
    gsl_vector_set(e->f_hi, i, p->f(e->x_hi, params));
    gsl_vector_set(e->x_hi, i, gsl_vector_get(x, i));
  }
  
  forward_difference_df(x, *f, params, df);
  forward_difference_d2f(x, *f, e->f_hi, params, d2f);
}
